#include <stdio.h>
#include <math.h>

//编译：gcc -o run_gauss_newton_fit_v1 gauss_newton_fit_v1.c -lm

#include <stdio.h>
#include <math.h>

// 二次曲线的模型函数
double model(double x, const double *params) {
    return params[0] * x * x + params[1] * x + params[2];
}

// 残差函数
double residual(double x, double y, const double *params) {
    return y - model(x, params);
}

// 高斯-牛顿迭代函数
void gauss_newton(double points[3][2], double *params) {
    double x[3] = {points[0][0], points[1][0], points[2][0]};
    double y[3] = {points[0][1], points[1][1], points[2][1]};
    double J[3][3]; // 雅可比矩阵
    double delta[3]; // 残差
    double inverse_J[3][3]; // 雅可比矩阵的逆矩阵
    double det_J; // 雅可比矩阵的行列式
    int iterations = 10; // 最大迭代次数
    double epsilon = 1e-6; // 收敛阈值

    // 初始化参数和雅可比矩阵
    double current_params[3] = {0, 0, 0}; // 初始参数
    double previous_params[3]; // 上一次迭代时的参数

    // 初始化逆矩阵和残差
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            J[i][j] = 0;
        }
        delta[i] = 0;
    }

    // 高斯-牛顿迭代
    for (int iter = 0; iter < iterations; ++iter) {
        // 计算残差
        for (int i = 0; i < 3; ++i) {
            delta[i] = residual(x[i], y[i], current_params);
        }

        // 计算雅可比矩阵
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                J[i][j] = 0;
            }
            for (int j = 0; j < 3; ++j) {
                J[i][j] += (i == j) ? 2 * current_params[0] : current_params[1];
            }
        }

        // 计算雅可比矩阵的逆矩阵
        det_J = J[0][0] * (J[1][1] * J[2][2] - J[1][2] * J[2][1] - J[1][0] * J[2][1] + J[2][0] * J[1][1]) +
                J[0][1] * (J[1][2] * J[2][0] - J[1][0] * J[2][2] - J[1][1] * J[2][0] + J[2][1] * J[1][0]) +
                J[0][2] * (J[1][0] * J[2][1] - J[1][1] * J[2][0] - J[1][0] * J[2][1] + J[2][0] * J[1][1]);

        if (fabs(det_J) < epsilon) {
            // 如果雅可比矩阵接近奇异，则退出迭代
            printf("雅可比矩阵接近奇异，迭代终止。\n");
            break;
        }

        // 更新参数
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                inverse_J[i][j] = (i == j) ? 1.0 / J[i][i] : 0;
            }
        }

                for (int i = 0; i < 3; ++i) {
            previous_params[i] = current_params[i];
            for (int j = 0; j < 3; ++j) {
                current_params[i] += inverse_J[i][j] * delta[j];
            }
        }

        // 打印当前参数
        printf("当前参数: a = %f, b = %f, c = %f\n", current_params[0], current_params[1], current_params[2]);

        // 检查是否收敛
        double change = 0;
        for (int i = 0; i < 3; ++i) {
            change += fabs(current_params[i] - previous_params[i]);
        }
        if (change < epsilon) {
            printf("参数收敛，迭代终止。\n");
            break;
        }
    }

}

int main() {
    // 给定的三个点
    double points[3][2] = {
        {79758, 19.43},
        {61358, 14.349},
        {40887, 9.491}
    };

    // 存储系数
    double coefficients[3] = {0, 0, 0};

    // 执行高斯-牛顿拟合
    gauss_newton(points, coefficients);

    // 打印结果
    printf(">>> y = %fx^2 + %fx + %f \n", coefficients[0], coefficients[1], coefficients[2]);

    float x = 166387.0;
    printf("x=%f so y=%f \r\n",x,((coefficients[0]*pow(x,2))+(coefficients[1]*x)+coefficients[2])/100000);

    return 0;
}
